#include <joyos.h>

// my headers
#include "define.h"
#include "goalscore.h"
#include "robomove.h"
#include "roboturn.h"
#include "led.h"
#include "bumper.h"
#include "distance.h"
#include "init.h"
#include "skunk.h"
#include "strategy.h"
#include "monitor.h"

	/*	#  is going to do something
		0  get away from opponent
		1  find side wall
		1  goto opponent goal 4
		2  walk pass the goal
		3  score goal 2
		6  turn at the opponent goal 1
		9  catch skunk
		12 turn after catching skunk
		15 score goal 1
		21 score goal 4
		24 go to opponent home
		27 done
	*/
//int start_time = 0;
//uint16_t uff_state_count = 0;

void myStrategy(void) {
	uint8_t wallSide;
	
	if (initPlace == 0) {
		// whiteStrategy
		wallSide = 1;
	}
	else if (initPlace == 1) {
		// blackStrategy
		wallSide = 0;
	}
	else {
		printf_P (PSTR("\nCannot choose\n choose white"));
		pause(2000);
		wallSide = 1;
	}
	
	myStateStatus = 0;		// 0 = in progress, 1 = finish the state

	while(state < 27){
		
		if (check_stuck == 1) {
			//robot hit into something --> check by monitor.c
			printf_P (PSTR("\nSTUCK: Robo cannot move front"));
					if(initPlace == 0){
						roboStraight(-100);
						pause(750);
						turnWithShaft(-60);
						roboStraight(100);
						pause(750);
						turnWithShaft(60);
						closeSkunk();
					}else{
						roboStraight(-100);
						pause(750);
						turnWithShaft(60);
						roboStraight(100);
						pause(750);
						turnWithShaft(-60);
						closeSkunk();
					}
		}else if (check_stuck == 3) {  // Turn Stuck 
			//robot hit into something --> check by monitor.c
			printf_P (PSTR("\nSTUCK: Robo cannot move back"));
					if(initPlace == 0){
						roboStraight(100);
						pause(750);
						turnWithShaft(-60);
						roboStraight(-100);
						pause(750);
						turnWithShaft(60);
					}else{
						roboStraight(100);
						pause(750);
						turnWithShaft(60);
						roboStraight(-100);
						pause(750);
						turnWithShaft(-60);						
					}
		}
		
		else if (state == 1) {
			// get close to side wall
			closeSkunk();
			
			printf_P (PSTR("\nGet Close to Wall"));
			if (!(wallLeftBumperPressed() || wallRightBumperPressed())) {
				wallFollow(wallSide, WALL_MOTOR_VEL);
				status = 0;
			}
			else {
				myStateStatus = 1;
			}
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				backToWallTimeout = 0;
				state = 4;
			}
		}
		
		if (state == 2) {
			// go back to wall (until back bumper preesed?)
			printf_P (PSTR("\nGo back to the wall"));
			
			goBack(250);
			myStateStatus = 1;
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 4;
			}
		}

		if (state == 4) {
			// go to opponent goal 4
			printf_P (PSTR("\nGoing to Opp Goal 4!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 5;
			}
		}
		
		else if (state == 5) {
			// walk pass opponent goal 4
			printf_P (PSTR("\nPass Opp Goal 4!"));
			walkPassGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 6;
			}
		}
	
		else if (state == 6) {
			// go to goal 2 and score it
			printf_P (PSTR("\nGoal 2 and Score!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				roboStop();
				myStateStatus = 0;
				// we are at goal 2
				printf_P (PSTR("\nAt 2 Scoring!"));
				
				
				score(wallSide, pointToDo[1]);
				
				state = 7;
			}
		}
		
		else if (state == 7) {
			// walk pass goal 2
			printf_P (PSTR("\nPass Goal 2!"));
			walkPassGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 8;
			}
		}
		
		else if (state == 8) {
			// go to the opponent 1-point bin and turn
			printf_P (PSTR("\n Opp Goal 1 and Turn!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				// we are at opponent goal 1
				printf_P (PSTR("\nAt Opp Goal 1!"));
				
				goBack(50);
				
				roboStop();
				
				if (wallSide == 1) {
					turnToOrient(2);
				}
				else {
					turnToOrient(0);
				}
				// pause(1000);
				state = 9;
			}
		}
		
		// skunk stuff here ( go straight to the wall then catch??)
		
		if (state == 9) {
			//adjust orientation
			printf_P (PSTR("\nAdjust Orientation"));
			
			/*
			if (!(wallLeftBumperPressed() || wallRightBumperPressed())) {
				wallFollow(wallSide, WALL_MOTOR_VEL);
			}
			goBack(200);
			turnWithShaft(40);
			*/
			
			if(wallSide == 1){ //Left
				motorSetVel(MOTOR_PORT_RIGHT, 100);
				pause(500);
			}else{
				motorSetVel(MOTOR_PORT_LEFT, 100);
				pause(500);	
			}
			
			roboStop();
			
			state = 10;
			
			}
		
		
		else if (state == 10) {
			//prepare for skunk
			printf_P (PSTR("\nPrepare for Skunk"));
			
			openSkunk();
			clearEncoder();		
			skunk_stop_enc = encoder_read(ENCODER_PORT_LEFT) + SKUNK_LEFT_ENC;
			state = 11;
		}
		
		else if (state == 11) {
		
			// walk to the wall and then catch
			check_stuck = 0;
			printf_P (PSTR("\nGo Stright and Catch Skunk!"));
			if (!(frontBumperPressed())) {
				//roboStraight(STRAIGHT_MOTOR_VEL);
				motorSetVel(MOTOR_PORT_RIGHT, 150);
				motorSetVel(MOTOR_PORT_LEFT, 150);
				status = 0;
				// lineFollow(LINE_MOTOR_VEL); --> choose 1
			}
			else if (encoder_read(ENCODER_PORT_LEFT) < skunk_stop_enc) {
				// meet enermy
				closeSkunk();
				oldState = state;
				enemyCount = 0;
				turnToOrient(3);
				roboStraight(ENEMY_MOTOR_VEL);
				state = 0;
				
			}
			else {
				// hit the wall
				closeSkunk();
				
				//goBack(250);
				
				state = 12;
			}
		}
		
		else if (state == 12) {
			// turn after catching skunk
			printf_P (PSTR("\nTurning to Orient 2!"));
			turnToOrient(3);
			state = 15;
						
		}
		
		else if (state == 15) {
			// go to goal 1 and score it
			printf_P (PSTR("\nGo to 1 and Score!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				// we are at goal 1
				printf_P (PSTR("\nAt 1!"));
				roboStop();
				score(wallSide, pointToDo[2]);
				state = 16;
							
			}
		}
		
		else if (state == 16) {
			// walk pass goal 1
			printf_P (PSTR("\nPass 1!"));
			walkPassGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 18;
							
			}
		}
	
		else if (state == 18) {
			// go to opponent goal 2
			printf_P (PSTR("\nGo to Opp 2!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 19;
								
			}
		}
		
		else if (state == 19) {
			// walk pass opponent goal 2
			printf_P (PSTR("\nPass Opp 2!"));
			walkPassGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 21;
			}
		}
	
		else if (state == 21) {
			// go to goal 4 and score it
			printf_P (PSTR("\nGo to 4 and Score!"));
			walkToGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				// we are at goal 4
				printf_P (PSTR("\nAt 4!"));
				roboStop();
				score(wallSide, pointToDo[0]);
				state = 22;
						
			}
		}
		
		else if (state == 22) {
			// walk pass goal 4
			printf_P (PSTR("\nPass 4!"));
			walkPassGoal(wallSide);
			
			if (myStateStatus == 1) {
				myStateStatus = 0;
				state = 24;
			}
		}
		
		// need more work here --> good for now
		else if (state == 24) {
			// go to opponent home
			printf_P (PSTR("\nGo to Opp Home!"));
			// if (not at opponent home) {
				wallFollow(wallSide, WALL_MOTOR_VEL);
				pause(5000);
			//}
			// else {
				state = 27;
								
			//}
		}
		
		else if (state == 0){
			// getting away from enermy
			clearEncoder();
			roboStraight(ENEMY_MOTOR_VEL);
			
			if (frontBumperPressed()) {
				if (enemyCount == 0) {
					enemyCount = 1;
						
					if (initPlace == 0){			//START ADDED
						goBack(600);
						turnLeft(90);
						if ((state < 9) || (state > 11)){
							walkToGoal(wallSide);
						}
						else{
							roboStraight(150);
							pause(500);
						}
						turnLeft(90);
					}
					else{
						goBack(600);
						turnRight(90);
						if ((state < 9) || (state > 11)){
							walkToGoal(wallSide);
						}
						else{
							roboStraight(150);
							pause(500);
						}
						turnRight(90);
					}								//END ADDED
					
					//turnRight(180);
				}
				else {
					// white Strategy
					state = oldState;
					if (state <= 6) {
						goBack(600);
						turnToOrient(1);
					}
			
					else if (state >= 7 && state <= 14) {
						if (wallSide == 1) {
							// white Strategy
							goBack(600);
							turnToOrient(2);
						}
						else {
							// black Strategy
							goBack(600);
							turnToOrient(0);
						}
					}
			
					else if (state >=15) {
						goBack(600);
						turnToOrient(3);
					}
				}
			}
		}else{
			//unfullfill_state[uff_state_count] = state;
			//uff_state_count++;
		}
		
		// check enermy
		
		if (foundEnemy()){
			oldState = state;
			state = 0;
			enemyCount = 0;
			
			if (state <= 6) {
				if (wallSide == 1) {
					// white Strategy
					turnToOrient(2);
				}
				else {
					// black Strategy
					turnToOrient(0);
				}
			}
			
			else if (state >= 7 && state <= 14) {
				turnToOrient(3);
			}
			
			else if (state >=15) {
				if (wallSide == 1) {
					// white Strategy
					turnToOrient(0);
				}
				else {
					// black Strategy
					turnToOrient(2);
				}
			}
			clearEncoder();
			roboStraight(ENEMY_MOTOR_VEL);
		}

		// if needed
		pause(10);
	}
	
	roboStop();
	printf_P (PSTR("\nDone We Win!"));
}

void clearEncoder(void) {
	encoder_reset(ENCODER_PORT_LEFT);
	encoder_reset(ENCODER_PORT_RIGHT);
}

void backToWall(){  // --> will be moved to strategy
	if ( backLeftBumperPressed() && backRightBumperPressed()) {
		myStateStatus = 1;
	}
	else if (backLeftBumperPressed()) {
		// left touch the wall
		motor_set_vel(MOTOR_PORT_LEFT, 120);
		motor_set_vel(MOTOR_PORT_RIGHT, -120);
	}
	
	else if (backRightBumperPressed()) {
		// right touch the wall
		motor_set_vel(MOTOR_PORT_LEFT, -120);
		motor_set_vel(MOTOR_PORT_RIGHT, 120);
	}
	
	else {
		roboStraight(-120);
	}
	
	// for time out
	backToWallTimeout ++;
	
	if (backToWallTimeout > 200) {
		printf_P (PSTR("\nBack To Wall : Timeout"));
		myStateStatus = 1;
	}
	
	status = 0;
}

void goBack (uint16_t duration) {
	// use time = 2 * duration
	/*
	for(int i =0;i<2;i++){
		motorSetVel(MOTOR_PORT_LEFT,-200);
		motorSetVel(MOTOR_PORT_RIGHT,0);
		pause((duration/2));		// let the robotcontinue to press against wall a little more
		motorSetVel(MOTOR_PORT_RIGHT,-200);
		motorSetVel(MOTOR_PORT_LEFT,0);
		pause((duration/2));
	}
	*/
	
	uint16_t i = 0;
	clearEncoder();
	
	while (i < duration) {
		roboStraight(-225);
		status = 0;
		pause(50);
		i += 50;
		printf("\ni = %d", i);
	}
	
	roboStop();
}

void walkToGoal(uint8_t wallwhere) {	// left : 1 --> right : 0
	if (isGoal(wallwhere) == 0) {
		wallFollow(wallwhere, WALL_MOTOR_VEL);
	}
	else {
		myStateStatus = 1;
	}
}

void walkPassGoal(uint8_t wallwhere) {	// left : 1 --> right : 0
	if (isGoal(wallwhere) == 1) {
		wallFollow(wallwhere, WALL_MOTOR_VEL);
	}
	else {
		myStateStatus = 1;
	}
}
